// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanningAndTrackingMetricsAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Runtime;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    
    /// <summary>
    /// Path planning and tracking metrics agent.
    /// </summary>
    /// <remarks>
    /// This agent sends a series of goal points to complete an "obstacle course."
    /// It watches drive feedback and localization; accumulating metrics such as total
    /// time, total rotational distance, total translation.
    /// </remarks>
    [DataContract]
    public class PathPlanningAndTrackingMetricsAgent :
        ConsumerProducerAgent<SimpleAgentMessage<DriveFeedbackState>, SimpleAgentMessage<PathPlanningAndTrackingMetrics>>,
        ISubscriptionReceiver<SimpleAgentMessage<PathPlan>>
    {
        /// <summary>
        /// Name of the agent producing path plans.
        /// </summary>
        [DataMember(Name = "PathPlanner")]
        private readonly string pathPlanProducer = default(string);

        /// <summary>
        /// Agent producing path plans.
        /// </summary>
        private IAgentHost pathPlanningAgent;

        /// <summary>
        /// Last received drive feedback.
        /// </summary>
        private SimpleAgentMessage<DriveFeedbackState> lastDriveFeedback;

        /// <summary>
        /// Total accumulated rotation in radians.
        /// </summary>
        private double totalRotationalDistance;

        /// <summary>
        /// Total accumulated translation distance in meters.
        /// </summary>
        private double totalTranslationDistance;

        /// <summary>
        /// Current deviation distance from the planned path.
        /// </summary>
        private double currentDeviationDistance;

        /// <summary>
        /// Total cumulative deviation distance over entire run.
        /// </summary>
        private double totalDeviationDistance;

        /// <summary>
        /// Total number of deviation distance samples over entire run.
        /// </summary>
        private int deviationDistanceSamples;

        /// <summary>
        /// Maximum deviation distance during entire run.
        /// </summary>
        private double maximumDeviationDistance;

        /// <summary>
        /// Total accumulated time.
        /// </summary>
        private TimeSpan totalTime;

        /// <summary>
        /// Current robot pose.
        /// </summary>
        private Pose2D robotPose;

        /// <summary>
        /// Robot heading.
        /// </summary>
        private double heading;

        /// <summary>
        /// Map distance resolution.
        /// </summary>
        private double mapDistanceResolution;

        /// <summary>
        /// Initializes a new instance of the PathPlanningAndTrackingMetricsAgent class.
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="driveFeedbackProducer">Name of the agent producing drive feedback messages.</param>
        /// <param name="pathPlanProducer">Name of the agent producing path plans.</param>
        public PathPlanningAndTrackingMetricsAgent(
            string name,
            IProducer<SimpleAgentMessage<DriveFeedbackState>> driveFeedbackProducer,
            IProducer<SimpleAgentMessage<PathPlan>> pathPlanProducer)
            : base(name, driveFeedbackProducer.Name)
        {
            this.pathPlanProducer = pathPlanProducer.Name;
        }

        /// <summary>
        /// Called when agent is initialized.
        /// </summary>
        /// <param name="locator">Agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            this.pathPlanningAgent = locator.GetAgent(this.pathPlanProducer);
            this.pathPlanningAgent.Subscribe<SimpleAgentMessage<PathPlan>>(this);
        }

        /// <summary>
        /// Called when the agent is activated.
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();

            this.totalRotationalDistance = 0;
            this.totalTranslationDistance = 0;
            this.totalTime = default(TimeSpan);
            this.totalDeviationDistance = 0;
            this.deviationDistanceSamples = 0;
            this.maximumDeviationDistance = 0;

            this.pathPlanningAgent.Activate();
        }

        /// <summary>
        /// Called when the agent is deactivated.
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            this.pathPlanningAgent.Deactivate();
        }

        /// <summary>
        /// Process received PathPlan messages.
        /// </summary>
        /// <param name="plan">PathPlan message.</param>
        public void Receive(SimpleAgentMessage<PathPlan> plan)
        {
            if (plan.State.Map != null)
            {
                this.robotPose = plan.State.CurrentPose;
                Microsoft.Robotics.Tracing.TraceOut.Info(Microsoft.Robotics.Tracing.TraceContexts.Nav, "RobotMapPoseAgent pose: {0}", this.robotPose);
                this.mapDistanceResolution = plan.State.Map.DistanceResolution;
            }

            if (plan.State.Path != null)
            {
                Point2D<int> location = PathPlanningCommon.PosePoint(this.robotPose);
                Point2D<int> nearest = PathPlanningCommon.NearestPoint(plan.State.Path, location);
                this.currentDeviationDistance = PathPlanningCommon.EuclideanDistance(location, nearest) * plan.State.Map.DistanceResolution;

                this.totalDeviationDistance += this.currentDeviationDistance;
                this.deviationDistanceSamples++;
                this.maximumDeviationDistance = Math.Max(this.currentDeviationDistance, this.maximumDeviationDistance);
            }

            PathPlanningAndTrackingMetrics metrics =
                new PathPlanningAndTrackingMetrics(
                    this.totalRotationalDistance,
                    this.totalTranslationDistance,
                    this.totalTime,
                    plan.State.CurrentPose,
                    this.currentDeviationDistance,
                    this.totalDeviationDistance / this.deviationDistanceSamples,
                    this.maximumDeviationDistance,
                    this.mapDistanceResolution);

            SimpleAgentMessage<PathPlanningAndTrackingMetrics> message =
                new SimpleAgentMessage<PathPlanningAndTrackingMetrics>(
                    metrics,
                    plan);

            this.Publisher.Post(message);
        }

        /// <summary>
        /// Process received drive feedback messages.
        /// </summary>
        /// <param name="driveFeedback">Drive feedback message.</param>
        public override void Receive(SimpleAgentMessage<DriveFeedbackState> driveFeedback)
        {
            if (this.lastDriveFeedback == null)
            {
                this.lastDriveFeedback = driveFeedback;
                return;
            }

            long ellapsedMilliseconds = driveFeedback.Timestamp - this.lastDriveFeedback.Timestamp;
            this.totalTime = this.totalTime.Add(TimeSpan.FromMilliseconds(ellapsedMilliseconds));

            DriveFeedbackState drive = driveFeedback.State;            
            Pose2D deltaPosition = drive.GetMeanDeltaPose(this.lastDriveFeedback.State);
            this.heading = Pose2D.ClampAngle(this.heading + deltaPosition.Heading);

            double x = this.robotPose.X;
            double y = this.robotPose.Y;
            double heading = this.heading;
            this.totalRotationalDistance += deltaPosition.Heading;
            this.totalTranslationDistance += deltaPosition.TotalTranslation();

            this.lastDriveFeedback = driveFeedback;
        }
    }
}
